﻿#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
#include <cmath>
#include "CircleFitSolver.h"
#include <QDebug>
using namespace std;
using namespace cv;

bool fitCircle(const std::vector<cv::Point2i> &points, double &center_x, double &center_y, double &radius)
{
     center_x = 0.0f;
     center_y = 0.0f;
     radius = 0.0f;
     if (points.size() < 3)
     {
         return false;
     }

     double sum_x = 0.0f, sum_y = 0.0f;
     double sum_x2 = 0.0f, sum_y2 = 0.0f;
     double sum_x3 = 0.0f, sum_y3 = 0.0f;
     double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

     int N = points.size();
     for (int i = 0; i < N; i++)
     {
         double x = points[i].x;
         double y = points[i].y;
         double x2 = x * x;
         double y2 = y * y;
         sum_x += x;
         sum_y += y;
         sum_x2 += x2;
         sum_y2 += y2;
         sum_x3 += x2 * x;
         sum_y3 += y2 * y;
         sum_xy += x * y;
         sum_x1y2 += x * y2;
         sum_x2y1 += x2 * y;
     }

     double C, D, E, G, H;
     double a, b, c;

     C = N * sum_x2 - sum_x * sum_x;
     D = N * sum_xy - sum_x * sum_y;
     E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
     G = N * sum_y2 - sum_y * sum_y;
     H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
     a = (H * D - E * G) / (C * G - D * D);
     b = (H * C - E * D) / (D * D - G * C);
     c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

     center_x = a / (-2);
     center_y = b / (-2);
     radius = sqrt(a * a + b * b - 4 * c) / 2;
     return true;
}


bool fitCircle(const cv::Mat &image, double &center_x, double &center_y, double &radius, unsigned char threshold)
{
    if( image.empty() )
    {
        center_x = 0;
        center_y = 0;
        radius = 0;
        return false;
    }

    cv::Mat grayImage;

    if(image.type() == CV_8UC4) // 暂时只支持 BGR 和 GRAY 两种颜色模式
    {
        cv::cvtColor( image, grayImage, CV_BGRA2GRAY );
    }
    else if(image.type() == CV_8UC3)
    {
        cv::cvtColor( image, grayImage, CV_BGR2GRAY );
    }
    else
    {
        grayImage = image.clone();
    }

    if( threshold == 0) // 正常的阈值分割不会用到 0 作为阈值
    {
        cv::threshold(grayImage, grayImage, 128, 255, CV_THRESH_OTSU);
    }
    else
    {
        cv::threshold(grayImage, grayImage, threshold, 255, CV_THRESH_BINARY);
    }
    //cv::threshold(grayImage, image, 128, 255, CV_THRESH_OTSU);
    //cv::imshow("image", image);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    cv::morphologyEx(image, image, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(image, image, cv::MORPH_CLOSE, kernel);
    cv::Canny(image, image, 128, 200, 3);

    std::vector<cv::Point2i> points;

    for(int row = 0; row < image.rows; row++)
    {
        const uchar * pLine = image.ptr<const uchar>(row);
        for(int col = 0; col < image.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }
    //radius = -1;
    //return true;
    return fitCircle(points, center_x, center_y, radius);
}


bool fitInnerCircle(const cv::Mat &image, double &center_x, double &center_y, double &radius, unsigned char threshold)
{
    if( image.empty() )
    {
        center_x = 0;
        center_y = 0;
        radius = 0;
        return false;
    }
    cv::Mat grayImage;

    if(image.type() == CV_8UC4) // 暂时只支持 BGR 和 GRAY 两种颜色模式
    {
        cv::cvtColor( image, grayImage, CV_BGRA2GRAY );
    }
    else if(image.type() == CV_8UC3)
    {
        cv::cvtColor( image, grayImage, CV_BGR2GRAY );
    }
    else
    {
        grayImage = image.clone();
    }

    //
    cv::Canny(grayImage, grayImage,  100, 200, 5);
    imshow("1", grayImage);

    std::vector<cv::Point2i> points;

    for(int row = 0; row < grayImage.rows; row++)
    {
        const uchar * pLine = grayImage.ptr<const uchar>(row);
        for(int col = 0; col < grayImage.cols; col++)
        {
            if(pLine[col] > 200)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }
    CircleFitSolver solver;
    radius = -1;
    bool ret = fitCircle(points, center_x, center_y, radius);
    // 对于圆环来说，这里拟合出的圆介于内外圆之间
    if( !ret )
    {
        return false;
    }
    // 抹去外圆
    std::vector<cv::Point2i> p2;
    int N = points.size();
    for(int i = 0; i < N; i++)
    {
        int col = points[i].x;
        int row = points[i].y;
        if(hypot(col - center_x, row - center_y) < radius)
        {
            p2.push_back(cv::Point2i(col, row));
        }
    }
    // 再次拟合就只剩下内圆了
    return solver.circleFitL1(p2, center_x, center_y, radius);
}
